package Behaviors;

import lejos.robotics.navigation.Pose;
import lejos.robotics.subsumption.Behavior;
import sharedData.DebugItem;
import sharedData.MapData;
import sharedData.SharedConstants;
import NXTExplorer.ExplorerBot;
import NXTExplorer.SensorParameter;

public class ArcSearch implements Behavior {

	private boolean suppressed = false;

	private SensorParameter paramObj;
	
	public ArcSearch(SensorParameter params) {
		paramObj = params;
	}

	@Override
	public boolean takeControl() {
		boolean takeControl = ExplorerBot.USSRMode > 0;
		if(takeControl)
			sendDebug("ArcSearch takes control");
			
		return takeControl;
	}

	@Override
	public void action() {

		suppressed = false;

		if (ExplorerBot.USSRMode == 1) {

			paramObj.getPilot().rotate(90);

			int backReading = 0;
			int frontReading = 0;

			// DEBUG
			sendDebug("Arc Searching for a left wall");
			
			paramObj.getPilot().travel(ExplorerBot.lastDistanceKnown + (int)(SharedConstants.PILOT_TRACKWIDTH));
			
			backReading = paramObj.getLeftUssr().getDistance();
			frontReading = paramObj.getCenterUssr().getDistance() - 3;

			int i = 0;

			while (backReading < frontReading) {

				i++;

				paramObj.getPilot().rotate(1);

				backReading = paramObj.getLeftUssr().getDistance();
				frontReading = paramObj.getCenterUssr().getDistance() - 3;
			}

			// DEBUG
			sendDebug("Corrected: " + i + " times, to the right");

			ExplorerBot.USSRMode = -1;

			handlePosition(paramObj.getPoseProvider().getPose());

		} else if (ExplorerBot.USSRMode == 2) {

			paramObj.getPilot().rotate(-90);

			int backReading = 0;
			int frontReading = 0;

			// DEBUG
			sendDebug("Arc Searching for a right wall");
			
			paramObj.getPilot().travel(ExplorerBot.lastDistanceKnown + (int)(SharedConstants.PILOT_TRACKWIDTH));

			backReading = paramObj.getRightUssr().getDistance();
			frontReading = paramObj.getCenterUssr().getDistance() - 3;

			int i = 0;

			while (backReading < frontReading) {

				i++;

				paramObj.getPilot().rotate(-1);

				backReading = paramObj.getRightUssr().getDistance();
				frontReading = paramObj.getCenterUssr().getDistance() - 3;
			}

			// DEBUG
			sendDebug("Corrected: " + i + " times, to the left");

			ExplorerBot.USSRMode = -2;

			handlePosition(paramObj.getPoseProvider().getPose());
		}

		//Thread.yield();
	}

	private void handlePosition(Pose pose) {
		if (pose == null)
			return;
		MapData data = new MapData(SharedConstants.DATA_TYPE_MAP_DATA,
				pose.getX(), pose.getY(), pose.getHeading(), paramObj
						.getLeftUssr().getRange(), paramObj.getRightUssr()
						.getRange(), paramObj.getCenterUssr().getRange());
		paramObj.getQueue().add(data);

	}

	@Override
	public void suppress() {
		suppressed = true;
	}

	public void sendDebug(String msg) {
		paramObj.getQueue().add(
				new DebugItem(SharedConstants.DATA_TYPE_DEBUG, paramObj
						.getPoseProvider().getPose().getX(), paramObj
						.getPoseProvider().getPose().getY(), paramObj
						.getPoseProvider().getPose().getHeading(), msg));
	}

}
